﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Incendios0_1.Camera.wpfdatamanager;
using Incendios0_1;
using AsyncCamera.Camera.Structure;
using Incendios0_1.tools;
using Incendios0_1.Camera.Pelco;
using System.Threading;
using Incendios0_1.localization;

namespace AsyncCamera.Camera.Pelco
{
    public class PelcoCamera: AbstractCamera
    {
        private static Vector3 defDirection = new Vector3(0, 1, 0);

        private static int stepHDegreeSpeed = 10;
        private static int stepVDegreeSpeed = 33;
        private static double instructionDelay = 300;
       

        public double? Latitud
        {
            get { return cameraData.Latitude; }
        }

        public double? Longitud
        {
            get { return cameraData.Longitude; }
        }

        public double? detectedLatitud = null;
        public double? detectedLongitud = null;


        private class AsyncCommand
        {
            public CameraOperations command;
            public PelcoCamera camera;
            public AsyncCommand(CameraOperations com, PelcoCamera cam)
            {
                command = com;
                camera = cam;
            }
        }

        private AsyncMessageSender<AsyncCommand> sender;

        public PelcoCamera(CameraData cameraData):base(cameraData)
        {
            sender = new AsyncMessageSender<AsyncCommand>(instructionDelay, SendControlSignal);
        }

        public PelcoCamera(CameraData cameraData, FilterTypes filter):base(cameraData)
        {

        }

        ~PelcoCamera()
        {
            Stop();
        }

        public override void Stop()
        {
            base.Stop();
            sender.Stop();
        }

        private void InitializeMedia()
        {
            cameraData.HSpeed = 50;
            cameraData.VSpeed = 50;
            cameraData.Preset = 0;
            cameraData.turbo = false;
        }

        public override void Reset()
        {
            cameraData.HSpeed = 50;
            cameraData.VSpeed = 50;
            cameraData.Preset = 0;
            cameraData.turbo = false;
        }

        private bool SendControlSignal(AsyncCommand command)
        {
            switch (command.command)
            {
                case CameraOperations.MoveUp:
                    SendCommand(CameraProtocol.MoveUp(command.camera));
                    break;
                case CameraOperations.MoveDown:
                    SendCommand(CameraProtocol.MoveDown(command.camera));
                    break;
                case CameraOperations.MoveLeft:
                    SendCommand(CameraProtocol.MoveLeft(command.camera));
                    break;
                case CameraOperations.MoveRight:
                    SendCommand(CameraProtocol.MoveRight(command.camera));
                    break;
                case CameraOperations.OpenIris:
                    SendCommand(CameraProtocol.OpenIris(command.camera));
                    break;
                case CameraOperations.CloseIris:
                    SendCommand(CameraProtocol.CloseIris(command.camera));
                    break;
                case CameraOperations.FocusFar:
                    SendCommand(CameraProtocol.FocusFar(command.camera));
                    break;
                case CameraOperations.FocusNear:
                    SendCommand(CameraProtocol.FocusNear(command.camera));
                    break;
                case CameraOperations.ZoomIn:
                    SendCommand(CameraProtocol.ZoomIn(command.camera));
                    break;
                case CameraOperations.ZoomOut:
                    SendCommand(CameraProtocol.ZoomOut(command.camera));
                    break;
                case CameraOperations.SetPreset:
                    SendCommand(CameraProtocol.FocusNear(command.camera));
                    break;
                case CameraOperations.CallPreset:
                    SendCommand(CameraProtocol.ZoomIn(command.camera));
                    break;
                case CameraOperations.ClearPreset:
                    SendCommand(CameraProtocol.ClearPreset(command.camera));
                    break;
                case CameraOperations.Wipe:
                    SendCommand(CameraProtocol.Wipe(command.camera));
                    break;
                case CameraOperations.Menu:
                    SendCommand(CameraProtocol.Menu(command.camera));
                    break;
                case CameraOperations.Stop:
                    SendCommand(CameraProtocol.Stop(command.camera));
                    break;
            }
            return true;
        }

        public void MoveUp()
        {
            sender += new AsyncCommand(CameraOperations.MoveUp, this);
        }

        public void MoveDown()
        {
            sender += new AsyncCommand(CameraOperations.MoveDown, this);
        }

        public void MoveUp(int grad)
        {
            int oldSpeed = cameraData.VSpeed;
            cameraData.VSpeed = stepVDegreeSpeed;
            SerialPortManager.cameraPort.Write(CameraProtocol.MoveUp(this), 0, 7);
            Thread.Sleep(new TimeSpan((long)(instructionDelay * grad * 10000)));
            SerialPortManager.cameraPort.Write(CameraProtocol.Stop(this), 0, 7);
            cameraData.VSpeed = oldSpeed;
        }

        public void MoveDown(int grad)
        {
            int oldSpeed = cameraData.VSpeed;
            cameraData.VSpeed = stepVDegreeSpeed;
            SerialPortManager.cameraPort.Write(CameraProtocol.MoveDown(this), 0, 7);
            Thread.Sleep(new TimeSpan((long)(instructionDelay * grad * 10000)));
            SerialPortManager.cameraPort.Write(CameraProtocol.Stop(this), 0, 7);
            cameraData.VSpeed = oldSpeed;
        }

        public void MoveLeft()
        {
            sender += new AsyncCommand(CameraOperations.MoveLeft, this);
        }

        public void MoveRight(int grad)
        {
            int oldSpeed = cameraData.HSpeed;
            cameraData.HSpeed = stepHDegreeSpeed;
            /*SerialPortManager.cameraPort.Write(CameraProtocol.MoveRight(this), 0, 7);
            Thread.Sleep(new TimeSpan((long)(instructionDelay * grad * 10000)));
            SerialPortManager.cameraPort.Write(CameraProtocol.Stop(this), 0, 7);
            cameraData.HSpeed = oldSpeed;*/
            for (int x = 0; x < grad; x++)
            {
                SendCommand(CameraProtocol.MoveRight(this));
                SendCommand(CameraProtocol.Stop(this));
                Thread.Sleep(500);
            }
            cameraData.HSpeed = oldSpeed;
        }

        public void MoveLeft(int grad)
        {
            int oldSpeed = cameraData.HSpeed;
            cameraData.HSpeed = stepHDegreeSpeed;
            SerialPortManager.cameraPort.Write(CameraProtocol.MoveLeft(this), 0, 7);
            Thread.Sleep(new TimeSpan((long)(instructionDelay * grad * 10000)));
            SerialPortManager.cameraPort.Write(CameraProtocol.Stop(this), 0, 7);
            cameraData.HSpeed = oldSpeed;
        }

        public void MoveRight()
        {
            sender += new AsyncCommand(CameraOperations.MoveRight, this);
        }

        public void ZoomIn()
        {
            sender += new AsyncCommand(CameraOperations.ZoomIn, this);
        }

        public void ZoomOut()
        {
            sender += new AsyncCommand(CameraOperations.ZoomOut, this);
        }

        public void FocusNear()
        {
            sender += new AsyncCommand(CameraOperations.FocusNear, this);
        }

        public void FocusFar()
        {
            sender += new AsyncCommand(CameraOperations.FocusFar, this);
        }

        public void OpenIris()
        {
            sender += new AsyncCommand(CameraOperations.OpenIris, this);
        }

        public void CloseIris()
        {
            sender += new AsyncCommand(CameraOperations.CloseIris, this);
        }

        public void MotionStop()
        {
            sender += new AsyncCommand(CameraOperations.Stop, this);
        }

        public void CallPreset()
        {
            sender += new AsyncCommand(CameraOperations.CallPreset, this);
        }

        public void SetPreset()
        {
            sender += new AsyncCommand(CameraOperations.SetPreset, this);
        }

        public void ClearPreset()
        {
            sender += new AsyncCommand(CameraOperations.ClearPreset, this);
        }

        public void Wipe()
        {
            sender += new AsyncCommand(CameraOperations.Wipe, this);
        }

        public void Menu()
        {
            sender += new AsyncCommand(CameraOperations.Menu, this);
        }

        public decimal readPan()
        {
            byte[] buffer = new byte[18];
            lock (SerialPortManager.cameraPort)
            {
                SerialPortManager.cameraPort.Write(CameraProtocol.querypan(this),0,7);
                SerialPortManager.cameraPort.DiscardInBuffer();
                Thread.Sleep(300);
                SerialPortManager.cameraPort.Read(buffer, 0, 18);
                return CameraProtocol.readCoord(buffer);
            }
        }

        public decimal readTilt()
        {
            byte[] buffer = new byte[18];
            lock (SerialPortManager.cameraPort)
            {
                SerialPortManager.cameraPort.Write(CameraProtocol.querytilt(this), 0, 7);
                SerialPortManager.cameraPort.DiscardInBuffer();
                Thread.Sleep(250);
                SerialPortManager.cameraPort.Read(buffer, 0, 18);
                return CameraProtocol.readCoord(buffer);
            }
        }

        public Tuple<decimal, decimal> readCoords()
        {
            decimal tilt = readTilt();
            decimal pan = readPan();
            
            return new Tuple<decimal, decimal>(pan, tilt);
        }

        public Vector3 getDirectionVector()
        {
            Vector3 dir = defDirection.Clone();
            Tuple<decimal, decimal> cc = readCoords();
            dir.Th += calculateTiltDeviation(cc.Item2);
            dir.Ph += calculatePanDeviation(cc.Item1);
            Vector3 pos = getSphereCoords();
            dir.Th += pos.Th;
            dir.Ph += pos.Ph;
            return dir;
        }

        public GeoCoord getGeoCoords()
        {
            return new GeoCoord((float)cameraData.Latitude, (float)cameraData.Longitude, (float)cameraData.Height);
        }

        public Vector3 getSphereCoords()
        {
            return new Vector3(Vector3.Geo2Sphere(getGeoCoords()));
        }

        private List<List<byte>> SendCommand(byte[] command)
        {
            SerialPortManager.cameraPort.Write(command, 0, 7);
            List<List<byte>> reply = ReadCamReply();
            reply.RemoveAt(0);
            return reply;
        }

        private List<List<byte>> ReadCamReply()
        {
            Int32 newByte = 0;
            List<List<byte>> reply = new List<List<byte>>();
            List<byte> replyLine = null;
            while (newByte >=0)
            {
                try
                {
                    newByte = SerialPortManager.cameraPort.ReadByte();
                }
                catch (TimeoutException)
                {
                    break;
                }
                catch (Exception e)
                {
                    Console.Write(e.Message);
                }

                if (newByte == CameraProtocol.BOL)
                {
                    replyLine = new List<byte>();
                    reply.Add(replyLine);
                }
                replyLine.Add(Convert.ToByte(newByte));
            }
            return reply;
        }

        public float calculatePanDeviation(decimal pan)
        {
            if (pan >= 0 && pan <= 180)
                return (float)pan;
            else if (pan > 180 && pan <= 360)
            {
                return (360 - (float)pan);
            }
            else throw new Exception("Unexpected pan inclination");
        }

        public float calculateTiltDeviation(decimal tilt)
        {
            return (float)-tilt;
        }

    }
}
